#include "Matrix.h"

#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) > (b) ? (b) : (a))
#define ACCURACY 1e-8

using namespace LinearAlgebra;

// 模板显式实例化，从而实现模板与实现的分离
template class DynamicArray<int>;
template class DynamicArray<Real>;

/**
 * @file Matrix.cpp
 * @author xingyifan
 * @date 2022-11-29 11:45
 *
 * @description: 成员函数定义
 */

// 初始化构造函数
Vector::Vector(int D, Real t) : m_dim(D)
{
	m_value = new Real[D];
	for (int i = 0; i < D; m_value[i++] = t)
		;
}

// 可以通过列表初始化
Vector::Vector(std::initializer_list<Real> l) : m_dim(l.size())
{
	m_value = new Real[m_dim];
	auto j = l.begin();
	for (int d = 0; d < m_dim; ++d)
		m_value[d] = *j++;
}

// 拷贝构造
Vector::Vector(const Vector &rhs) : m_dim(rhs.size())
{
	m_value = new Real[m_dim];
	for (int d = 0; d < m_dim; d++)
		m_value[d] = rhs.m_value[d];
}

// 获得向量长度
int Vector::size() const
{
	return m_dim;
}

// 返回引用
Real &Vector::operator[](int i)
{
	assert(i < m_dim);
	return m_value[i];
}

// 返回值
Real Vector::operator[](int i) const
{
	assert(i < m_dim);
	return m_value[i];
}

// 向量加减
Vector Vector::operator+(const Vector &rhs) const
{
	Vector res = *this;
	for (int i = 0; i < m_dim; i++)
		res[i] += rhs[i];
	return res;
}
Vector Vector::operator-(const Vector &rhs) const
{
	Vector res = *this;
	for (int i = 0; i < m_dim; i++)
		res[i] -= rhs[i];
	return res;
}

// 常数乘除法
Vector Vector::operator*(Real t) const
{
	Vector res = *this;
	for (int d = 0; d < m_dim; ++d)
		res[d] *= t;
	return res;
}
Vector Vector::operator/(Real t) const
{
	assert(fabs(t) > ACCURACY);
	Vector res = *this;
	for (int d = 0; d < m_dim; ++d)
		res[d] /= t;
	return res;
}

// 赋值
void Vector::operator=(const Vector &rhs)
{
	assert(m_dim == rhs.m_dim);
	for (int d = 0; d < m_dim; d++)
		m_value[d] = rhs[d];
}

// 取反操作
Vector Vector::operator-() const
{
	Vector res = *this;
	for (int d = 0; d < m_dim; ++d)
		res[d] *= -1;
	return res;
}

// 冒泡排序
void Vector::sort()
{
	for (int i = 0; i < m_dim; i++)
	{
		for (int j = 1; j < m_dim; j++)
		{
			// 比较相邻的元素，如果后者小于前者就交换
			if (m_value[j] < m_value[j - 1])
			{
				Real tmp = m_value[j];
				m_value[j] = m_value[j - 1];
				m_value[j - 1] = tmp;
			}
		}
	}
}

// 反转向量
Vector Vector::reverse()
{
	Vector res(m_dim);
	for (int i = 0; i < m_dim; i++)
		res[i] = m_value[m_dim - i - 1];
	return res;
}

// 元素求和
Real Vector::sum() const
{
	Real a = 0;
	for (int i = 0; i < m_dim; i++)
		a += m_value[i];
	return a;
}

// 取绝对值
Vector Vector::abs() const
{
	Vector res(m_dim);
	for (int d = 0; d < m_dim; ++d)
		res[d] = fabs(m_value[d]);
	return res;
}

// 计算范数
Real Vector::norm(int nt) const
{
	if (nt == 2)
	{
		return sqrt(dot(*this, *this));
	}
	else if (nt == 1)
	{
		Real a = 0;
		for (int i = 0; i < m_dim; ++i)
			a += fabs(m_value[i]);
		return a;
	}
	else if (nt == 0)
	{
		Real a = fabs(m_value[0]);
		for (int i = 1; i < m_dim; i++)
		{
			a = MAX(fabs(m_value[i]), a);
		}
		return a;
	}
	return 0;
}

// 取最小元
Real Vector::Min() const
{
	Real a = m_value[0];
	for (int d = 1; d < m_dim; d++)
		if (m_value[d] < a)
			a = m_value[d];
	return a;
}

// 取最大元
Real Vector::Max() const
{
	Real a = m_value[0];
	for (int d = 1; d < m_dim; d++)
		if (m_value[d] > a)
			a = m_value[d];
	return a;
}

// 取最大模的分量
int Vector::maxIndex() const
{
	int index = 0;
	for (int d = 1; d < m_dim; d++)
		index = (fabs(m_value[d]) > fabs(m_value[index]) ? d : index);
	return index;
}

// 转化成矩阵
Matrix Vector::toMatrix() const
{
	Matrix m(m_dim, 1);
	for (int i = 0; i < m_dim; i++)
		m(i, 0) = m_value[i];
	return m;
}

// 转置
Matrix Vector::transpose() const
{
	Matrix m(1, m_dim);
	for (int i = 0; i < m_dim; i++)
		m(0, i) = m_value[i];
	return m;
}

// 释放资源
Vector::~Vector()
{
	delete[] m_value;
}

// 初始化构造函数
Matrix::Matrix(int Row, int Col, Real t) : m_row(Row), m_col(Col)
{
	m_value = new Real *[Row];
	for (int i = 0; i < Row; i++)
	{
		m_value[i] = new Real[Col];
		for (int j = 0; j < Col; j++)
			m_value[i][j] = t;
	}
}

// 通过列表初始化
Matrix::Matrix(int Row, int Col, std::initializer_list<Real> l) : m_row(Row), m_col(Col)
{
	assert(l.size() == Row * Col);
	m_value = new Real *[Row];
	auto it = l.begin();
	for (int i = 0; i < Row; i++)
	{
		m_value[i] = new Real[Col];
		for (int j = 0; j < Col; j++)
		{
			m_value[i][j] = *it;
			*it++;
		}
	}
}

// 拷贝构造
Matrix::Matrix(const Matrix &rhs) : m_row(rhs.m_row), m_col(rhs.m_col)
{
	m_value = new Real *[m_row];
	for (int i = 0; i < m_row; i++)
	{
		m_value[i] = new Real[m_col];
		for (int j = 0; j < m_col; j++)
			m_value[i][j] = rhs.m_value[i][j];
	}
}

// 获得行数
int Matrix::row() const
{
	return m_row;
}

// 获得列数
int Matrix::col() const
{
	return m_col;
}

// 返回第 j 列
Vector Matrix::operator()(int j) const
{
	assert(j < m_col);
	Vector v(m_row);
	for (int i = 0; i < m_row; i++)
		v[i] = m_value[i][j];
	return v;
}

// 返回 i,j 元素
Real Matrix::operator()(int i, int j) const
{
	assert(i < m_row && j < m_col);
	return m_value[i][j];
}

// 返回 i,j 元素引用
Real &Matrix::operator()(int i, int j)
{
	assert(i < m_row && j < m_col);
	return m_value[i][j];
}

// 矩阵加减乘
Matrix Matrix::operator+(const Matrix &rhs) const
{
	assert(m_row == rhs.m_row && m_col == rhs.m_col);
	Matrix m = *this;
	for (int i = 0; i < m_row; i++)
		for (int j = 0; j < m_col; j++)
			m(i, j) += rhs(i, j);
	return m;
}
Matrix Matrix::operator-(const Matrix &rhs) const
{
	assert(m_row == rhs.m_row && m_col == rhs.m_col);
	Matrix m = *this;
	for (int i = 0; i < m_row; i++)
		for (int j = 0; j < m_col; j++)
			m(i, j) -= rhs(i, j);
	return m;
}

// 矩阵乘法
Matrix Matrix::operator*(const Matrix &rhs) const
{
	assert(m_col == rhs.m_row);
	int Col2 = rhs.m_col;
	Matrix m(m_row, Col2);
	for (int i = 0; i < m_row; i++)
	{
		for (int j = 0; j < Col2; j++)
		{
			Real sum = 0;
			for (int k = 0; k < m_col; k++)
				sum += m_value[i][k] * rhs(k, j);
			m(i, j) = sum;
		}
	}
	return m;
}
Vector Matrix::operator*(const Vector &v) const
{
	assert(m_col == v.size());
	Vector res(m_row);
	for (int i = 0; i < m_row; i++)
	{
		Real sum = 0;
		for (int j = 0; j < m_col; j++)
			sum += m_value[i][j] * v[j];
		res[i] = sum;
	}
	return res;
}
Matrix Matrix::operator*(Real t) const
{
	Matrix m = *this;
	for (int i = 0; i < m_row; i++)
		for (int j = 0; j < m_col; j++)
			m(i, j) *= t;
	return m;
}
Matrix Matrix::operator/(Real t) const
{
	assert(fabs(t) > ACCURACY);

	Matrix m = *this;
	for (int i = 0; i < m_row; i++)
		for (int j = 0; j < m_col; j++)
			m(i, j) /= t;
	return m;
}

// 直接赋值
void Matrix::operator=(const Matrix &rhs)
{
	assert(m_row == rhs.m_row && m_col == rhs.m_col);
	for (int i = 0; i < m_row; i++)
		for (int j = 0; j < m_col; j++)
			m_value[i][j] = rhs(i, j);
}

// 取反操作
Matrix Matrix::operator-() const
{
	Matrix m = *this;
	for (int i = 0; i < m_row; i++)
		for (int j = 0; j < m_col; j++)
			m(i, j) *= -1;
	return m;
}

// 在 j 列插入列向量 v
void Matrix::insert(int j, Vector &v)
{
	assert(m_row == v.size() && j < m_col);
	for (int i = 0; i < m_row; i++)
		m_value[i][j] = v[i];
}

// 计算范数
Real Matrix::norm(int nt) const
{
	const Matrix &lhs = *this;
	if (nt == 1)
	{
		Real max = 0;
		for (int i = 0; i < m_col; i++)
		{
			Real a = 0;
			for (int j = 0; j < m_row; a += fabs(lhs(j++, i)))
				;
			max = MAX(a, max);
		}
		return max;
	}
	else if (nt == 0)
	{
		Real max = 0;
		for (int i = 0; i < m_row; i++)
		{
			Real a = 0;
			for (int j = 0; j < m_col; a += fabs(lhs(i, j++)))
				;
			max = MAX(a, max);
		}
		return max;
	}
	return 0;
}

// e 范数 / f 范数
Real Matrix::norm(char c) const
{
	const Matrix &lhs = *this;
	if (c == 'e')
	{
		Real sum = 0;
		for (int i = 0; i < m_row; i++)
			for (int j = 0; j < m_col; j++)
				if (i != j)
					sum += lhs(i, j) * lhs(i, j);
		return sqrt(sum);
	}
	else if (c == 'f')
	{
		Real sum = 0;
		for (int i = 0; i < m_row; i++)
			for (int j = 0; j < m_col; j++)
				sum += lhs(i, j) * lhs(i, j);
		return sqrt(sum);
	}
	return 0;
}

// 获取上三角
Matrix Matrix::upper() const
{
	const Matrix &A = *this;
	Matrix m(m_row, m_col);
	int limit = MIN(m_row, m_col);
	for (int i = 0; i < limit; i++)
		for (int j = i; j < limit; j++)
			m(i, j) = A(i, j);
	return m;
}

// 获得下三角
Matrix Matrix::lower() const
{
	const Matrix &A = *this;
	Matrix m(m_row, m_col);
	int limit = MIN(m_row, m_col);
	for (int i = 0; i < limit; i++)
		for (int j = 0; j < i + 1; j++)
			m(i, j) = A(i, j);
	return m;
}

// 转置矩阵
Matrix Matrix::transpose() const
{
	const Matrix &A = *this;
	Matrix m(m_col, m_row);
	for (int i = 0; i < m_col; i++)
		for (int j = 0; j < m_row; j++)
			m(i, j) = A(j, i);
	return m;
}

// 矩阵求逆
Matrix Matrix::inverse() const
{
	assert(m_row == m_col);

	// 初始化单位阵
	Matrix m(m_row, m_row);
	for (int i = 0; i < m_row; i++)
		m(i, i) = 1;

	// 保存当前矩阵
	Matrix old = *this;

	Real max;  // 最大值
	int index; // 最大值位置

	for (int i = 0; i < m_row; i++)
	{
		max = 0;
		index = i;

		// 寻找最大列元作为主元
		for (int j = i; j < m_row; j++)
		{
			Real tmp = fabs(old(j, i));
			if (tmp > max)
			{
				max = tmp;
				index = j;
			}
		}

		// 确保主元足够大
		assert(max > ACCURACY);

		// 交换最大元到对角位置
		for (int j = 0; j < m_row; j++)
		{
			Real tmp = old(i, j);
			old(i, j) = old(index, j);
			old(index, j) = tmp;

			tmp = m(i, j);
			m(i, j) = m(index, j);
			m(index, j) = tmp;
		}

		// Gauss 消去
		for (int j = 0; j < m_row; j++)
		{
			Real tmp = old(i, i);
			if (i == j)
			{
				for (int k = 0; k < m_row; k++)
				{
					m(j, k) /= tmp;
					old(j, k) /= tmp;
				}
				continue;
			}
			tmp = old(j, i) / old(i, i);
			for (int k = 0; k < m_row; k++)
			{
				m(j, k) -= tmp * m(i, k);
				old(j, k) -= tmp * old(i, k);
			}
		}
	}

	return m;
}

// LU 分解
Matrix Matrix::LU() const
{
	assert(m_row == m_col);
	Matrix A = *this;

	for (int i = 0; i < m_row; i++)
	{
		Real key = A(i, i);

		// 确保对角元足够大
		assert(fabs(key) > ACCURACY);

		// 第 i 列高斯变换
		for (int j = i + 1; j < m_row; j++)
		{
			A(j, i) /= key;
			for (int k = i + 1; k < m_row; k++)
				A(j, k) -= A(i, k) * A(j, i);
		}
	}
	return A;
}

// 列主元 LU 分解，传入下标数组，返回置换后的数组
Matrix Matrix::LU(int perm[]) const
{
	assert(m_row == m_col);
	Matrix A = *this;

	Real max, tmp; // 记录最大值
	int index;	   // 记录要交换的主元位置

	for (int i = 0; i < m_row; i++)
	{
		max = 0;
		index = i;

		// 选取列主元
		for (int p = i; p < m_row; p++)
		{
			tmp = fabs(A(p, i));
			if (tmp > max)
			{
				max = tmp;
				index = p;
			}
		}

		// 确保主元足够大
		assert(max > ACCURACY);

		// 交换主元
		for (int q = 0; q < m_row; q++)
		{
			tmp = A(i, q);
			A(i, q) = A(index, q);
			A(index, q) = tmp;
		}

		// 交换指标，记录置换
		int tmp_ind = perm[i];
		perm[i] = perm[index];
		perm[index] = tmp_ind;

		// 正常的高斯消去法
		for (int j = i + 1; j < m_row; j++)
		{
			A(j, i) /= A(i, i);
			for (int k = i + 1; k < m_row; k++)
				A(j, k) -= A(i, k) * A(j, i);
		}
	}

	return A;
}

// Cholesky
Matrix Matrix::Cholesky() const
{
	assert(m_row == m_col);
	Matrix A = *this;

	for (int i = 0; i < m_row; i++)
	{
		Real sum = 0;
		for (int p = 0; p < i; p++)
			sum += A(i, p) * A(i, p);

		A(i, i) = sqrt(A(i, i) - sum);

		// 确保对角元足够大
		assert(fabs(A(i, i) > ACCURACY));

		for (int j = i + 1; j < m_row; j++)
		{
			sum = 0;
			for (int k = 0; k < i; k++)
				sum += A(j, k) * A(i, k);

			A(j, i) = (A(j, i) - sum) / A(i, i);
			A(i, j) = A(j, i);
		}
	}

	return A;
}

// 优化的 Cholesky
Matrix Matrix::CholeskyD() const
{
	assert(m_row == m_col);
	Matrix A = *this;

	for (int i = 0; i < m_row; i++)
	{
		Real sum = 0;
		for (int p = 0; p < i; p++)
			sum += A(i, p) * A(i, p) * A(p, p);

		A(i, i) -= sum;

		// 确保对角元足够大
		assert(fabs(A(i, i) > ACCURACY));

		for (int j = i + 1; j < m_row; j++)
		{
			sum = 0;
			for (int k = 0; k < i; k++)
				sum += A(j, k) * A(i, k) * A(k, k);

			A(j, i) -= sum;
			A(j, i) /= A(i, i);
			A(i, j) = A(j, i);
		}
	}

	return A;
}

// QR 分解
Matrix Matrix::QR(Vector &d) const
{
	assert(d.size() == m_col);
	Matrix A = *this;

	for (int i = 0; i < m_col; i++)
	{
		Vector x(m_row - i);
		Vector v(m_row - i);

		for (int j = i; j < m_row; j++)
			x[j - i] = A(j, i);

		Real beta = Householder(x, v);
		Vector w(m_col - i);

		// get w = beta * AT * v
		for (int k = i; k < m_col; k++)
		{
			Real sum = 0;
			for (int p = i; p < m_row; p++)
				sum += A(p, k) * v[p - i];

			// note: it is w[k-i], not w[k]
			w[k - i] = beta * sum;
		}

		// get HA = A - v * wT
		for (int k = i; k < m_row; k++)
		{
			for (int p = i; p < m_col; p++)
			{
				if (p == i && k > i)
					A(k, p) = v[k - i];
				else
					A(k, p) -= v[k - i] * w[p - i];
			}
		}

		d[i] = beta;
	}

	return A;
}

// 上 Hessenberg 化，在 Q 中累计正交变换
Matrix Matrix::Hessenberg(Matrix &Q) const
{
	assert(m_row == m_col && Q.row() == Q.col() && m_row == Q.row());
	Matrix A = *this;

	int n = m_row;
	for (int i = 0; i < n - 2; i++)
	{
		// 用于保存 Householder 变换
		Vector x(n - i - 1);
		Vector v(n - i - 1);

		// x 取 A 的 i 列对角元以下的部分
		for (int j = i + 1; j < n; j++)
			x[j - i - 1] = A(j, i);

		// 为了节约，将 beta 存放在 v 头部
		Real beta = Householder(x, v);
		v[0] = beta;

		// 注意列的范围要从 0 到 n，不然右上角的元素无法更新
		A.HAH(i + 1, n, v);

		// 将 Q 对角元以下的部分赋值为 v
		for (int j = i + 1; j < n; j++)
			Q(j, i) = v[j - i - 1];
	}

	return A;
}

// 计算指定范围的 Householder 变换，作用结果为 HAH
void Matrix::HAH(int startOfRow, int endOfRow, Vector &v)
{
	Matrix &A = *this;
	int n = A.row();
	assert(n == A.col());

	// 先获得变换的变量
	Real beta = v[0];
	v[0] = 1;

	// 逐列计算 HA
	for (int j = 0; j < n; j++)
	{
		Real sum = 0;
		// 计算 beta * vT * Aj
		for (int k = startOfRow; k < endOfRow; k++)
			sum += beta * v[k - startOfRow] * A(k, j);

		// 计算 Aj - v * beta * vT * Aj
		for (int k = startOfRow; k < endOfRow; k++)
			A(k, j) -= sum * v[k - startOfRow];
	}

	// 逐行计算 HAH
	for (int j = 0; j < n; j++)
	{
		Real sum = 0;
		// 计算 beta * ATj * v
		for (int k = startOfRow; k < endOfRow; k++)
			sum += beta * v[k - startOfRow] * A(j, k);

		// 计算 ATj - beta * ATj * v * vT
		for (int k = startOfRow; k < endOfRow; k++)
			A(j, k) -= sum * v[k - startOfRow];
	}

	// 最后还原 v[0]
	v[0] = beta;
}

// 双重步 QR 迭代
void Matrix::doubleQR(int l, int m)
{
	assert(m_row == m_col && l < m_row && m < m_col);

	Matrix &A = *this;
	Real s = A(m_row - 2, m_row - 2) + A(m_row - 1, m_row - 1);
	Real t = A(m_row - 2, m_row - 2) * A(m_row - 1, m_row - 1) - A(m_row - 2, m_row - 1) * A(m_row - 1, m_row - 2);

	Vector x(3);
	x[0] = A(0, 0) * A(0, 0) + A(0, 1) * A(1, 0) - s * A(0, 0) + t;
	x[1] = A(1, 0) * (A(0, 0) + A(1, 1) - s);
	x[2] = A(1, 0) * A(2, 1);
	// 不考虑 n<3 的情况，即 A 至少是 3 阶矩阵

	// 计算 P0 * H * P0
	Vector v(3);
	Real beta = Householder(x, v);
	v[0] = beta;
	HAH(l, l + 3, v);

	// 开始双重步迭代
	x[0] = A(l + 1, l);
	x[1] = A(l + 2, l);
	for (int i = l + 1; i < m_row - m - 2; i++)
	{
		x[2] = A(i + 2, i - 1);
		beta = Householder(x, v);
		v[0] = beta;
		HAH(i, i + 3, v);

		x[0] = A(i + 1, i);
		x[1] = A(i + 2, i);
	}

	// 最后一步只有 2 阶
	Vector y({x[0], x[1]});
	Vector v2(2);
	beta = Householder(y, v2);
	v2[0] = beta;
	HAH(m_row - m - 2, m_row - m, v2);
}

// 释放资源
Matrix::~Matrix()
{
	for (int j = 0; j < m_row; j++)
		if (m_value[j] != nullptr)
			delete[] m_value[j];
	delete[] m_value;
}

// 构造函数
template <class T>
DynamicArray<T>::DynamicArray() : m_head(nullptr), m_tail(nullptr), m_size(0)
{
}

template <class T>
DynamicArray<T>::DynamicArray(std::initializer_list<T> l) : m_size(l.size())
{
	auto j = l.begin();
	m_head = new Node(*j++, nullptr, nullptr);

	Node *p = m_head;
	for (int i = 1; i < m_size; i++)
	{
		p->next = new Node(*j++, p, nullptr);
		p = p->next;
	}
	m_tail = p;
}

template <class T>
DynamicArray<T>::DynamicArray(const DynamicArray<T> &arr) : m_size(arr.m_size)
{
	m_head = (arr.empty() ? nullptr : new Node(arr[0], nullptr, nullptr));

	Node *p = m_head;
	for (int i = 1; i < m_size; i++)
	{
		p->next = new Node(arr[i], p, nullptr);
		p = p->next;
	}
	m_tail = p;
}

// 拷贝赋值
template <class T>
void DynamicArray<T>::operator=(const DynamicArray<T> &arr)
{
	// 首先清空
	this->clear();

	// 然后依次推入
	Node *node = arr.m_head;
	while (node != nullptr)
	{
		this->push_back(node->value);
		node = node->next;
	}
}

// 获取元素，如果超出数组范围，就创建新的节点达到该范围
template <class T>
T &DynamicArray<T>::operator[](int i)
{
	// 如果数组是空的，就先初始化
	if (m_size == 0)
	{
		m_head = new Node(T(), nullptr, nullptr);
		m_tail = m_head;
		m_size++;
	}

	// 如果在范围内，就从头部出发寻找
	Node *p = nullptr;
	if (i < m_size)
	{
		p = m_head;
		while (i > 0)
		{
			p = p->next;
			i--;
		}
	}
	// 如果超出范围，直接从尾部出发
	else
	{
		p = m_tail;
		// 注意要先减去数组范围
		i -= m_size - 1;
		while (i > 0)
		{
			p->next = new Node(T(), p, nullptr);
			p = p->next;
			m_size++;
			i--;
		}
	}

	// 如果这是最后一个节点，那么就作为尾部节点
	if (p->next == nullptr)
		m_tail = p;
	return p->value;
}

// 在数组范围内获取元素
template <class T>
T DynamicArray<T>::operator[](int i) const
{
	assert(i < m_size);
	Node *p = m_head;
	while (i > 0)
	{
		p = p->next;
		i--;
	}
	return p->value;
}

// 获得头部元素
template <class T>
T DynamicArray<T>::front() const
{
	return m_head->value;
}

// 获得尾部元素
template <class T>
T DynamicArray<T>::back() const
{
	return m_tail->value;
}

// 获得指定的节点
template <class T>
typename DynamicArray<T>::Node *DynamicArray<T>::node(int i) const
{
	assert(i < m_size);
	Node *p = m_head;
	while (i > 0)
	{
		p = p->next;
		i--;
	}
	return p;
}

// 在 i 位置插入元素
template <class T>
void DynamicArray<T>::insert(int i, T value)
{
	// 如果超出范围，就直接调用引用修改
	if (i >= m_size)
	{
		operator[](i) = value;
		return;
	}

	Node *q = new Node(value, nullptr, nullptr);
	if (i == 0)
	{
		q->next = m_head;

		if (m_head != nullptr)
		{
			m_head->prev = q;
			m_head = q;
		}
		else
		{
			// 数组是空
			m_head = q;
			m_tail = q;
		}
	}
	else
	{
		// 获取前一个位置的元素和后一个位置
		Node *prev = m_head;
		while (i - 1 > 0)
		{
			i--;
			prev = prev->next;
		}
		Node *next = prev->next;

		// 重新建立连接
		prev->next = q;
		q->prev = prev;
		q->next = next;

		// 如果 q 不是尾部
		if (next != nullptr)
			next->prev = q;
		else
			m_tail = q;
	}
	m_size++;
}

// 在指定节点处插入元素
template <class T>
typename DynamicArray<T>::Node *DynamicArray<T>::insert(Node *node, T value)
{
	Node *q = new Node(value, node->prev, node);

	// 如果是头部节点，就需要修改头部
	if (node == m_head)
		m_head = q;
	else
		node->prev->next = q;

	node->prev = q;
	m_size++;

	return q;
}

// 尾部推入元素
template <class T>
void DynamicArray<T>::push_back(T value)
{
	Node *q = new Node(value, nullptr, nullptr);

	// 如果数组是空，直接作为头尾节点
	if (m_size == 0)
	{
		m_head = q;
		m_tail = q;
	}
	else
	{
		q->prev = m_tail;
		m_tail->next = q;
		m_tail = q;
	}
	m_size++;
}

// 头部推入元素
template <class T>
void DynamicArray<T>::push_front(T value)
{
	Node *q = new Node(value, nullptr, nullptr);

	// 如果数组是空，直接作为头尾节点
	if (m_size == 0)
	{
		m_head = q;
		m_tail = q;
	}
	else
	{
		q->next = m_head;
		m_head->prev = q;
		m_head = q;
	}
	m_size++;
}

// 弹出最后一个元素
template <class T>
T DynamicArray<T>::pop_back()
{
	assert(m_size > 0);
	T value;

	if (m_size == 1)
	{
		value = m_head->value;
		delete m_head;
		m_head = nullptr;
		m_tail = nullptr;
	}
	else
	{
		value = m_tail->value;
		m_tail = m_tail->prev;
		delete m_tail->next;
		m_tail->next = nullptr;
	}
	m_size--;

	return value;
}

// 弹出第一个元素
template <class T>
T DynamicArray<T>::pop_front()
{
	assert(m_size > 0);
	T value;

	if (m_size == 1)
	{
		value = m_head->value;
		delete m_head;
		m_head = nullptr;
		m_tail = nullptr;
	}
	else
	{
		value = m_head->value;
		m_head = m_head->next;
		delete m_head->prev;
		m_head->prev = nullptr;
	}
	m_size--;

	return value;
}

// 找到元素
template <class T>
int DynamicArray<T>::find(T value) const
{
	int index = 0;
	Node *p = m_head;
	while (p != nullptr)
	{
		if (p->value == value)
			return index;
		p = p->next;
		index++;
	}
	return -1;
}

// 删除指定元素
template <class T>
bool DynamicArray<T>::kill(T value)
{
	Node *p = m_head;
	while (p != nullptr)
	{
		if (p->value == value)
		{
			// 头尾的特殊情况
			if (p == m_head)
			{
				this->pop_front();
				return true;
			}
			if (p == m_tail)
			{
				this->pop_back();
				return true;
			}

			// 其余情况正常删除
			Node *prev = p->prev;
			Node *next = p->next;
			prev->next = next;
			next->prev = prev;
			delete p;

			m_size--;
			return true;
		}
		p = p->next;
	}
	return false;
}

// 删除指定位置的元素
template <class T>
T DynamicArray<T>::remove(int i)
{
	assert(i < m_size);

	// 先找到节点
	Node *p = m_head;
	while (i > 0)
	{
		p = p->next;
		i--;
	}

	// 头尾的特殊情况
	if (p == m_head)
		return this->pop_front();
	if (p == m_tail)
		return this->pop_back();

	// 其余情况正常删除
	Node *prev = p->prev;
	Node *next = p->next;
	prev->next = next;
	next->prev = prev;

	T value = p->value;
	delete p;
	m_size--;
	return value;
}

// 移除指定节点的元素
template <class T>
void DynamicArray<T>::remove(Node *node)
{
	T value = node->value;

	// 头尾的特殊情况
	if (node == m_head)
	{
		this->pop_front();
		return;
	}
	if (node == m_tail)
	{
		this->pop_back();
		return;
	}

	// 其余情况正常删除
	Node *prev = node->prev;
	Node *next = node->next;
	prev->next = next;
	next->prev = prev;

	delete node;
	m_size--;
}

// 清空数组
template <class T>
void DynamicArray<T>::clear()
{
	Node *p = m_head;
	Node *q = nullptr;
	while (p != nullptr)
	{
		q = p;
		p = p->next;
		delete q;
	}

	// 初始化成员变量
	m_head = nullptr;
	m_tail = nullptr;
	m_size = 0;
}

// 析构函数
template <class T>
DynamicArray<T>::~DynamicArray()
{
	clear();
}

// 构造函数
SpMatrix::SpMatrix(int Row, int Col) : m_row(Row), m_col(Col)
{
	// 初始非零元都是对角元
	int nnz = MIN(Row, Col);

	// 对角元赋值零
	for (int i = 0; i < Row; i++)
		m_IA.push_back(i);
	for (int i = 0; i < nnz; i++)
	{
		m_JA.push_back(i);
		m_AA.push_back(0);
	}

	// 边界值
	m_JA.push_back(INT_MAX);
	m_IA.push_back(nnz);
	m_AA.push_back(FLT_MAX);
}

// 拷贝构造
SpMatrix::SpMatrix(const SpMatrix &spm) : m_row(spm.m_row), m_col(spm.m_col), m_IA(spm.m_IA), m_JA(spm.m_JA), m_AA(spm.m_AA)
{
}

// 用普通矩阵构造
SpMatrix::SpMatrix(const Matrix &m) : SpMatrix(m.row(), m.col())
{
	for (int i = 0; i < m_row; i++)
		for (int j = 0; j < m_col; j++)
			if (fabs(m(i, j)) > ACCURACY)
				this->operator()(i, j) = m(i, j);
}

// 获得行数
int SpMatrix::row() const
{
	return m_row;
}

// 获得列数
int SpMatrix::col() const
{
	return m_col;
}

// 获得非零元个数
int SpMatrix::explicitNum() const
{
	return m_IA.back();
}

// 获取对角元素
Real SpMatrix::diag(int i) const
{
	return m_AA[m_IA[i]];
}

// 获取指定行的开头
int SpMatrix::startOfRow(int i) const
{
	return m_IA[i];
}

// 通过全局指标获取元素所在的列位置
int SpMatrix::indexOfCol(int j) const
{
	return m_JA[j];
}

// 通过全局指标获取元素值
Real SpMatrix::global(int j) const
{
	return m_AA[j];
}

// 获取(i,j)元素的全局列标
int SpMatrix::globalCol(int i, int j) const
{
	DynamicArray<int>::Node *node_IA = m_IA.node(i);
	DynamicArray<int>::Node *node_JA = m_JA.node(node_IA->value);
	for (int k = node_IA->value; k < node_IA->next->value; k++)
	{
		if (node_JA->value == j)
			return k;
		node_JA = node_JA->next;
	}
	return -1;
}

// 重载以获得指定位置的元素，只能获得存在的元素，不存在的元素认为是零
Real SpMatrix::operator()(int i, int j) const
{
	assert(i < m_row && j < m_col);

	DynamicArray<int>::Node *node_IA = m_IA.node(i);
	DynamicArray<int>::Node *node_JA = m_JA.node(node_IA->value);
	for (int k = node_IA->value; k < node_IA->next->value; k++)
	{
		if (node_JA->value == j)
			return m_AA[k];
		node_JA = node_JA->next;
	}

	return 0;
}

// 设置指定位置的元素，不存在就创建新元素的引用返回
Real &SpMatrix::operator()(int i, int j)
{
	assert(i < m_row && j < m_col);

	// 这一行没有实际用处，只是用来返回值，实际上并不会用到
	DynamicArray<Real>::Node *node_AA = m_AA.node(0);

	// 这一行的范围
	DynamicArray<int>::Node *node_IA = m_IA.node(i);
	int start = node_IA->value;
	int end = node_IA->next->value;

	// 如果找到了就结束
	DynamicArray<int>::Node *node_JA = m_JA.node(start);
	DynamicArray<int>::Node *tmp = node_JA;
	for (int k = start; k < end; k++)
	{
		if (tmp->value == j)
			return m_AA[k];
		tmp = tmp->next;
	}

	// 没有找到，要插入一个新的元素，返回它的引用
	node_JA = node_JA->next;
	int index = start + 1;
	for (int k = start; k < end + 1; k++)
	{
		// 遇到第一个大于 j 的位置/这一行只有对角元/到达末尾时插入列标
		if (index == end || node_JA->value > j || k == end)
		{
			// 能够这样插入是因为最后还有一个元素占位，否则动态数组不能插入到最后一个元素位置
			m_JA.insert(node_JA, j);

			// 插入一个零
			node_AA = m_AA.insert(m_AA.node(index), 0);

			// 更新 IA，i 后面每个行标的开头都要后移
			node_IA = node_IA->next;
			for (int p = i + 1; p < m_row + 1; p++)
			{
				node_IA->value++;
				node_IA = node_IA->next;
			}

			return node_AA->value;
		}
		index = k + 1;

		// index == end 和 node_JA->value > j 两种情况插入位置相同
		if (k != start)
			node_JA = node_JA->next;
	}

	return node_AA->value;
}

// 压缩矩阵的零元素，返回移除的零元素数量
SpMatrix SpMatrix::compress() const
{
	SpMatrix spm(m_row, m_col);

	// 第 i 行的起始位置
	DynamicArray<int>::Node *node_IA = m_IA.node(0);
	for (int i = 0; i < m_row; i++)
	{
		int start = node_IA->value;
		int end = node_IA->next->value;

		// 第 i 行的列标开始循环赋值
		DynamicArray<int>::Node *node_JA = m_JA.node(start);
		for (int j = start; j < end; j++)
		{
			Real value = this->operator()(i, node_JA->value);

			// 只有当值足够大才进行赋值
			if (fabs(value) > ACCURACY)
				spm(i, node_JA->value) = value;
			node_JA = node_JA->next;
		}

		node_IA = node_IA->next;
	}

	return spm;
}

// 转置矩阵
SpMatrix SpMatrix::transpose() const
{
	SpMatrix spm(m_row, m_col);

	// 第 i 行的起始位置
	DynamicArray<int>::Node *node_IA = m_IA.node(0);
	for (int i = 0; i < m_row; i++)
	{
		int start = node_IA->value;
		int end = node_IA->next->value;

		// 第 i 行的列标开始循环赋值
		DynamicArray<int>::Node *node_JA = m_JA.node(start);
		for (int j = start; j < end; j++)
		{
			// 反转赋值
			spm(node_JA->value, i) = this->operator()(i, node_JA->value);
			node_JA = node_JA->next;
		}

		node_IA = node_IA->next;
	}

	return spm;
}

// 转化为一般矩阵
Matrix SpMatrix::toMatrix() const
{
	Matrix m(m_row, m_col);
	for (int i = 0; i < m_row; i++)
		for (int j = 0; j < m_col; j++)
			m(i, j) = this->operator()(i, j);
	return m;
}

// 加减法
SpMatrix SpMatrix::operator+(const SpMatrix &spm) const
{
	assert(m_row == spm.m_row && m_col == spm.m_col);

	SpMatrix res = *this;

	// 第 i 行的起始位置
	DynamicArray<int>::Node *node_IA = spm.m_IA.node(0);
	for (int i = 0; i < m_row; i++)
	{
		int start = node_IA->value;
		int end = node_IA->next->value;

		// 第 i 行的列标开始循环赋值
		DynamicArray<int>::Node *node_JA = spm.m_JA.node(start);
		for (int j = start; j < end; j++)
		{
			res(i, node_JA->value) += spm(i, node_JA->value);
			node_JA = node_JA->next;
		}

		node_IA = node_IA->next;
	}

	return res;
}
SpMatrix SpMatrix::operator-(const SpMatrix &spm) const
{
	assert(m_row == spm.m_row && m_col == spm.m_col);

	SpMatrix res = *this;

	// 第 i 行的起始位置
	DynamicArray<int>::Node *node_IA = spm.m_IA.node(0);
	for (int i = 0; i < m_row; i++)
	{
		int start = node_IA->value;
		int end = node_IA->next->value;

		// 第 i 行的列标开始循环赋值
		DynamicArray<int>::Node *node_JA = spm.m_JA.node(start);
		for (int j = start; j < end; j++)
		{
			res(i, node_JA->value) -= spm(i, node_JA->value);
			node_JA = node_JA->next;
		}

		node_IA = node_IA->next;
	}

	return res;
}

// 乘除法运算
SpMatrix SpMatrix::operator*(Real k) const
{
	SpMatrix res = *this;
	DynamicArray<Real>::Node *node_AA = res.m_AA.node(0);

	// 除了最后一个元素以外都乘 k
	for (int i = 0; i < m_AA.size() - 1; i++)
	{
		node_AA->value *= k;
		node_AA = node_AA->next;
	}
	return res;
}
SpMatrix SpMatrix::operator/(Real k) const
{
	assert(fabs(k) > ACCURACY);

	SpMatrix res = *this;
	DynamicArray<Real>::Node *node_AA = res.m_AA.node(0);

	// 除了最后一个元素以外都乘 k
	for (int i = 0; i < m_AA.size() - 1; i++)
	{
		node_AA->value /= k;
		node_AA = node_AA->next;
	}
	return res;
}

// 稀疏矩阵的乘法
SpMatrix SpMatrix::operator*(const SpMatrix &spm) const
{
	assert(m_col == spm.m_col);
	int Col2 = spm.m_col;
	SpMatrix res(m_row, Col2);

	DynamicArray<int>::Node *node_IA = m_IA.node(0);
	for (int i = 0; i < m_row; i++)
	{
		// 暂存数据
		Real *tmp_val = new Real[Col2];
		for (int j = 0; j < Col2; j++)
			tmp_val[j] = 0;

		// 根据 IA 获得 i 行元素中非零的列标索引 IA[i]，对应的列标是 JA[IA[i]]
		DynamicArray<int>::Node *node_JA = m_JA.node(node_IA->value);

		// 计算 i 行的结果
		for (int j = node_IA->value; j < node_IA->next->value; j++)
		{
			// 获得对应 JA[IA[i]] 列的 spm 的 JA[IA[i]] 行元素中非零列标索引
			DynamicArray<int>::Node *spm_node_IA = spm.m_IA.node(node_JA->value);

			//这是一个很巧妙的计算方式，只需要每次计算两个元素的乘积，记录在对应的列的位置上
			//然后不断累加即可，相当于将n次乘法和加法分开计算
			for (int k = spm_node_IA->value; k < spm_node_IA->next->value; k++)
				tmp_val[spm.m_JA[k]] += m_AA[j] * spm.m_AA[k];

			node_JA = node_JA->next;
		}

		// 去掉过小的元素
		for (int j = 0; j < Col2; j++)
			if (fabs(tmp_val[j]) > ACCURACY)
				res(i, j) = tmp_val[j];

		delete[] tmp_val;
		node_IA = node_IA->next;
	}
	return res;
}

// 拷贝赋值
void SpMatrix::operator=(const SpMatrix &spm)
{
	assert(m_row == spm.m_row && m_col == spm.m_col);
	m_AA = spm.m_AA;
	m_IA = spm.m_IA;
	m_JA = spm.m_JA;
}

// 向量点积
Real LinearAlgebra::dot(const Vector &vec1, const Vector &vec2)
{
	assert(vec1.size() == vec2.size());
	Real sum = 0;
	for (int i = 0; i < vec1.size(); i++)
		sum += vec1[i] * vec2[i];
	return sum;
}

// Householder 变换
Real LinearAlgebra::Householder(Vector &tx, Vector &v)
{
	// 使用无穷范数规范化防止溢出
	Real eta = tx.norm(0);
	Vector x = tx / eta;
	v = x;
	v[0] = 0;
	Real sigma = dot(x, x) - x[0] * x[0];

	Real beta = 0;
	if (fabs(sigma) > ACCURACY)
	{
		Real alpha = sqrt(x[0] * x[0] + sigma);
		if (x[0] <= 0)
			v[0] = x[0] - alpha;
		// 如果是正的分量，则用特殊算法减小舍入误差
		else
			v[0] = -sigma / (x[0] + alpha);

		// 对 beta 乘 v[0] * v[0] ，抵消规格化为 1 的影响
		beta = 2 * v[0] * v[0] / (sigma + v[0] * v[0]);
		v = v / v[0];
	}
	return beta;
}

// Givens 变换
Vector LinearAlgebra::Givens(Vector &v, int i, int j)
{
	Real sqrtX = sqrt(v[i] * v[i] + v[j] * v[j]);
	if (sqrtX == 0)
		return {1, 0};
	return {v[i] / sqrtX, v[j] / sqrtX};
}

// 希尔伯特矩阵
Matrix LinearAlgebra::Hilbert(int n)
{
	Matrix A(n, n);
	for (int i = 0; i < n; i++)
		for (int j = 0; j < n; j++)
			A(i, j) = 1.0 / (i + j + 1);

	return A;
}